#include "ActionController.h"
#define MAX_BALLOON 30

void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange);

/*
 This is the ActionTurn constructor, note the use of constructor chaining
 with the ArAction. also note how it uses setNextArgument, which makes
 it so that other things can see what parameters this action has, and
 set them.  It also initializes the classes variables.
 */
ActionController::ActionController(double stopDistance, double controlAngle,
		double velocityOnTarget, ActionGo *go, ActionTurn *turn, ActionScouting *scout) :
	ArAction("Controller") {
	myStopDistance = stopDistance;
	myControlAngle = controlAngle;
	myVelocityOnTarget = velocityOnTarget;
	myGo = go;
	myTurn = turn;
	myScout=scout;
	target=false;
	//targetold=false;
	num_target=0;
	//oldRange=stopDistance;
	connectToCam(1);

}

/*
 Sets the myRobot pointer (all setRobot overloaded functions must do this),
 finds the sonar device from the robot, and if the sonar isn't there,
 then it deactivates itself.
 */
void ActionController::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (mySonar == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

/*
 This is the guts of the Turn action.
 */
ArActionDesired *ActionController::fire(ArActionDesired currentDesired) {

	double range;
	double degree;
	Balloon targets[MAX_BALLOON];
	double degreeRange=50;

	myDesired.reset();
	// if the sonar is null we can't do anything, so deactivate
	if (mySonar == NULL) {
		deactivate();
		return NULL;
	}
	// get the range of the sonar
	range = mySonar->currentReadingPolar(-myControlAngle, myControlAngle)- myRobot->getRobotRadius();

	//acquisizione di un frame da CAM e controllo della presenza di obiettivi
	IplImage *src = connectToCam(0);
	num_target = getTargets(src, targets,cvPoint(src->width / 2, src->height));

	if (num_target > 0) {
		int near = nearest(targets, num_target);
		degree = targets[near].angle;
	} else
		degree = 0;

	if (range < myStopDistance) {
		bool isY=isYellow(src,cvPoint(src->width / 2, src->height),144,128,10);
				if (isY==false) {
						printf("Ho classificato l'oggetto come: ostacolo\n");

						if (myRobot->findAction("Scouting") != NULL){
							myRobot->remAction(myScout);
							myScout->left=false;
							myScout->right=false;
						}
						if (myRobot->findAction("Turn") == NULL)
							myRobot->addAction(myTurn, 60);

						if(myTurn->myTurning>0)
							myScout->sign=-1;
						else
							myScout->sign=1;

						if(num_target>0){
							if(myTurn->myTurning>0)
								myScout->sign=-1;
							else
								myScout->sign=1;
							myScout->leftControl=90;
							myScout->rightControl=270;
						}else{
							if(myTurn->myTurning>0)
								myScout->sign=-1;
							else
								myScout->sign=1;
							myScout->leftControl=-1;
							myScout->rightControl=150;
						}



				} else {
					printf("Ho classificato l'oggetto come: Target\n");
					if (myRobot->findAction("Go") != NULL)
						myRobot->remAction(myGo);

					myDesired.setVel(myVelocityOnTarget);
				  }
	} else if (range > myStopDistance) {
		if(num_target>0){
					printf("raggiungi target \n");

					if (myRobot->findAction("Turn") != NULL)
						myRobot->remAction(myTurn);

					if (myRobot->findAction("Go") == NULL)
						myRobot->addAction(myGo, 50);

					analizeDegree(&myDesired,degree,degreeRange);
		}else{
			printf("Sto in espolarazione \n");

			if (myRobot->findAction("Turn") != NULL)
				myRobot->remAction(myTurn);

			if (myRobot->findAction("Scouting") == NULL){
				myScout->startAngle=myRobot->getOdometerDegrees();
				myRobot->addAction(myScout, 50);
			}
		}


	}
	// return a pointer to the actionDesired, so resolver knows what to do

	return &myDesired;
}

void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange){

	if (degree > degreeRange) {
		myDesired->setVel(0);
		myDesired->setDeltaHeading(-10);
	} else if (degree < -degreeRange) {
				myDesired->setVel(0);
				myDesired->setDeltaHeading(10);
			} else
				myDesired->setDeltaHeading(0);

}
